#ifndef I_LIB_IP_I_GRADIENT_H
#define I_LIB_IP_I_GRADIENT_H

#include <cstdio>
#include <cassert>
#include <algorithm>
#include <vector>

#include "../core/i_blas.h"

namespace idl {
    /*Compute horizontal sobel operator  [1/2 0 -1/2] of an image scanline l and store in d.*/
    inline void i_sobel_dx_h(const unsigned char *l, int *d, int n) {
        int i = 1;
        int nm1 = n - 1;
        *d++ = (l[0] - l[1]) >> 1;
        for (; i < nm1; i++) {
            *d++ = (l[i - 1] - l[i + 1]) >> 1;
        }
        *d = (l[nm1 - 1] - l[nm1]) >> 1;
    }

    /*Compute vertical sobel operator [1 2 1]^t >> 2 to n entries in (s1,s2,s3) and store result in d*/
    inline void i_sobel_dx_v(const int *s1, const int *s2, const int *s3, int *d, int n) {
        int i = 0;
        int c = (n >> 4);
        for (; i < c; i++) {
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;

            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;

            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;

            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
        }
        if (n & 15) {
            if (n & 8) {
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;

                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            } 
            if (n & 4) {
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            }
            if (n & 2) {
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            }
            if (n & 1) {
                *d++ = ((*s1++) + ((*s2++) << 1) + (*s3++) + 2) >> 2;
            }
        }
    }

    /*Compute horizontal sobel operator [1,0,-1], then vertical sobel operator [1 2 1]^t >> 2 and store results in dx
    This function needs (height x width) entries of scratch space in ss*/
    inline void i_sobel_dx(const unsigned char *const *image, int **dx, int **ss, 
        int width, int height) {
        assert(height > 1 && width > 1);
        int i = 0;
        for (i = 0; i < height; i++) {
            i_sobel_dx_h(image[i], ss[i], width);
        }
        i_sobel_dx_v(ss[0], ss[0], ss[1], dx[0], width);
        for (i = 1; i < height - 1; ++i) {
            i_sobel_dx_v(ss[i-1], ss[i], ss[i+1], dx[i], width);
        }
        i_sobel_dx_v(ss[i-1], ss[i], ss[i], dx[i], width);
    }
    
    /*Compute horizontal sobel derivative [1 2 1]>>2 to n entries and store result in d.*/
    inline void i_sobel_dy_h(const unsigned char *l, int *d, int n) {
        int i = 1;
        int nm1 = n - 1;
        *d++ = (l[0] + (l[0] << 1) + l[1] + 2) >> 2;
        for (; i < nm1; i++) {
            *d++ = (l[i-1] + (l[i] << 1) + l[i+1] + 2) >> 2;
        }
        *d = (l[i - 1] + (l[i] << 1) + l[i] + 2) >> 2;
    }

    /*Compute vertical sobel derivative [1 0 -1]^t >> 1 to n entries in (s1,s2) and store result in d.*/
    inline void i_sobel_dy_v(const int *s1, const int* s2, int *d, int n) {
        int i = 0;
        int c = (n >> 4);
        for (; i < c; i++) {
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
                   
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
                   
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
                   
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
            *d++ = ((*s1++) - (*s2++)) >> 1;
        }
        if (n & 15) {
            if (n & 8) {
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;

                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
            }
            if (n & 4) {
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
            }
            if (n & 2) {
                *d++ = ((*s1++) - (*s2++)) >> 1;
                *d++ = ((*s1++) - (*s2++)) >> 1;
            }
            if (n & 1) {
                *d++ = ((*s1++) - (*s2++)) >> 1;
            }
        }
    }

    /*Compute horizontal sobel operator [1,2,1]>>2, then vertical sobel operator [1 0 -1]^t and store results in dy
    This function needs (height x width) entries of scratch space in ss*/
    inline void i_sobel_dy(const unsigned char *const *image, int **dy, int **ss,
        int width, int height) {
        assert(height > 1 && width > 1);
        int i;
        for (i = 0; i < height; i++) {
            i_sobel_dy_h(image[i], ss[i], width);
        }
        i_sobel_dy_v(ss[0], ss[1], dy[0], width);
        for (i = 1; i < height - 1; ++i) {
            i_sobel_dy_v(ss[i - 1], ss[i + 1], dy[i], width);
        }
        i_sobel_dy_v(ss[i - 1], ss[i], dy[i], width);
    }

    /*This function needs(height x width) entries of scratch space in ss*/
    inline void i_sobel_dxdy(const unsigned char *const *image, int **dx, int **dy, int **ss,
        int width, int height) {
        i_sobel_dx(image, dx, ss, width, height);
        i_sobel_dy(image, dy, ss, width, height);
    }

    inline void i_gradient_mag(const int *dx, const int *dy, int *mag, int size) {
        assert(dx != NULL && dy != NULL && mag != NULL);
        int i = 0;
        for (; i < size; ++i) {
            mag[i] = i_round(i_sqrt(i_sqr(dx[i]) + i_sqr(dy[i])));
        }
    }

    inline void i_gradient_mag_fast(const int *dx, const int *dy, int *mag, int size) {
        assert(dx != NULL && dy != NULL && mag != NULL);
        int i = 0;
        for (; i < size; ++i) {
            mag[i] = i_abs(dx[i]) + i_abs(dy[i]);
        }
    }

    inline void i_gradient_dir(const int *dx, const int *dy, float *dir, int size) {
        assert(dx != NULL && dy != NULL && dir != NULL);
        int i = 0;
        for (; i < size; ++i) {
            dir[i] = i_atan2((float)dy[i], (float)dx[i]);
        }
    }

    /*This function query the gradient direction using a 2D LUT (gridient values 0-510), lut[y][x] = atan2(y-255, x-255),
    the image gradient buffer dx, dy each contains n entries*/
    inline void i_gradient_dir_lut(const int *dx, const int *dy, 
        const float *const *lut, 
        float *dir, int n) {
        assert(dx != NULL && dy != NULL && dir != NULL);
        int i = 0;
        for (; i < n; ++i) {
            //assert(i_abs(dy[i]) < 256 && i_abs(dx[i]) < 256);
            dir[i] = lut[dy[i]+255][dx[i]+255];
        }
    }

    inline void i_gradient_nms_line(const int *mag1, const int *mag2, const int *mag3,
        const float *dir, unsigned char* label, int mag_thre, unsigned char val, int n) {
        assert(n && mag1 != NULL && mag2 != NULL && mag3 != NULL && dir != NULL && label != NULL);
        int x = 0;
        int mag_cen = 0; 
        int nm1 = n - 1;
        float deg;
        label[0] = 0;
        for (x = 1; x < nm1; x++) {
            mag_cen = mag2[x];
            if (mag_cen < mag_thre) {
                label[x] = 0;
                continue;
            }
            deg = i_radians_to_degree(dir[x]);
            if (deg >= 0) {
                if (deg < 22.5f) {
                    label[x] = (mag_cen > mag2[x - 1]) && (mag_cen > mag2[x + 1]) ? val : 0;
                } else if (deg < 67.5f) {
                    label[x] = (mag_cen > mag1[x - 1]) && (mag_cen > mag3[x + 1]) ? val : 0;
                } else if (deg < 112.5f) {
                    label[x] = (mag_cen > mag1[x]) && (mag_cen > mag3[x]) ? val : 0;
                } else if (deg < 157.5f) {
                    label[x] = (mag_cen > mag1[x + 1]) && (mag_cen > mag3[x - 1]) ? val : 0;
                } else {
                    label[x] = (mag_cen > mag2[x - 1]) && (mag_cen > mag2[x + 1]) ? val : 0;
                }
            } else {
                if (deg > -22.5f) {
                    label[x] = (mag_cen > mag2[x - 1]) && (mag_cen > mag2[x + 1]) ? val : 0;
                } else if (deg > -67.5f) {
                    label[x] = (mag_cen > mag1[x + 1]) && (mag_cen > mag3[x - 1]) ? val : 0;
                } else if (deg > -112.5f) {
                    label[x] = (mag_cen > mag1[x]) && (mag_cen > mag3[x]) ? val : 0;
                } else if (deg > -157.5f) {
                    label[x] = (mag_cen > mag1[x - 1]) && (mag_cen > mag3[x + 1]) ? val : 0;
                } else {
                    label[x] = (mag_cen > mag2[x - 1]) && (mag_cen > mag2[x + 1]) ? val : 0;
                }
            }
        }
        label[nm1] = 0;
    }

    inline void i_gradient_nms(const int *const *mag, const float *const *dir,
        unsigned char* const *label,
        int mag_thre, unsigned char val, int width, int height) {
        int y;
        i_gradient_nms_line(mag[0], mag[0], mag[1], dir[0], label[0], mag_thre, val, width);
        for (y = 1; y < height-1; ++y) {
            i_gradient_nms_line(mag[y-1], mag[y], mag[y+1], dir[y], label[y], mag_thre, val, width);
        }
        i_gradient_nms_line(mag[y-1], mag[y], mag[y], dir[y], label[y], mag_thre, val, width);
    }
} //namespace idl
#endif